#include "cordic.h"
#include "Arduino.h"
#include "stm32g4xx_hal_cordic.h"



CORDIC_HandleTypeDef hcordic;

void HAL_CORDIC_MspInit(CORDIC_HandleTypeDef* hcordic)
{
    if(hcordic->Instance == CORDIC)
    {
        __HAL_RCC_CORDIC_CLK_ENABLE();
    }
}

void CORDIC_Init(void)
{
    // Enable CORDIC clock
    __HAL_RCC_CORDIC_CLK_ENABLE();
    
    // Initialize CORDIC peripheral
    hcordic.Instance = CORDIC;
    if (HAL_CORDIC_Init(&hcordic) != HAL_OK)
    {
        Error_Handler();
    }
    
    // Configure CORDIC using HAL configuration
    CORDIC_ConfigTypeDef sCordicConfig = {0};
    
    sCordicConfig.Function = CORDIC_FUNCTION_SINE;     // sine function
    sCordicConfig.Precision = CORDIC_PRECISION_6CYCLES;// max precision
    sCordicConfig.Scale = CORDIC_SCALE_0;              // no scale
    sCordicConfig.NbWrite = CORDIC_NBWRITE_1;          // One input data
    sCordicConfig.NbRead = CORDIC_NBREAD_1;            // Two output data: sine then cosine
    sCordicConfig.InSize = CORDIC_INSIZE_16BITS;       // q15 format for input data
    sCordicConfig.OutSize = CORDIC_OUTSIZE_16BITS;     // q15 format for output data

    if (HAL_CORDIC_Configure(&hcordic, &sCordicConfig) != HAL_OK)
    {
        Error_Handler();
    }
}

void CORDIC_CalcCosSin(float angle, volatile float *cos_out, volatile float *sin_out)
{
    // We'll still use direct register access for calculation
    // but with the HAL configuration
    
    uint32_t cordicin = 0x7fff0000;  // magnitude = 1 in Q15 format
    int16_t thetashort = (int16_t)(angle * 10430.0f); // Scale to CORDIC format
    
    // Combine magnitude and angle
    cordicin += (uint16_t)thetashort;
    
    // Write input to CORDIC
    CORDIC->WDATA = cordicin;
    
    //delay 5us
    delayMicroseconds(5);

    // Read result (contains both sine and cosine)
    uint32_t out0 = CORDIC->RDATA;
    int16_t cos_val = (int16_t)((out0 >> 16) & 0xFFFF);   


    // Extract sine and cosine values with proper masking
    int16_t sin_val = (int16_t)((out0) & 0xFFFF);
    
    
    // Store results with temporary variables to ensure atomic updates
    float cos_temp = (float)cos_val / 32768.0f;
    float sin_temp = (float)sin_val / 32768.0f;
    
    *cos_out = cos_temp;
    *sin_out = sin_temp;
}